package org.opentcs.strategies.basic.routing.jgrapht.jjwcalg;


import lombok.Getter;
import lombok.Setter;
import org.jgrapht.Graph;
import org.jgrapht.GraphPath;
import org.jgrapht.Graphs;
import org.jgrapht.alg.shortestpath.ListSingleSourcePathsImpl;
import org.jgrapht.alg.util.ToleranceDoubleComparator;
import org.jgrapht.graph.GraphWalk;
import org.jheaps.AddressableHeap;
import org.jheaps.tree.PairingHeap;
import org.opentcs.components.kernel.routing.Edge;
import org.opentcs.data.model.TCSResource;
import org.opentcs.data.model.TCSResourceReference;
import org.opentcs.strategies.basic.scheduling.ReservationPool;

import java.util.*;
import java.util.function.Supplier;

/**
 * f(n) = g(n) +h(n)
 */
public class FSYAStartShortPathAlg<s, e> implements JJWCShortestPathAlgorithm<java.lang.String, org.opentcs.components.kernel.routing.Edge> {

  protected final Supplier<AddressableHeap<Double, String>> heapSupplier;
  protected AddressableHeap<Double, String> openList;
  protected Map<String, AddressableHeap.Handle<Double, String>> vertexToHeapNodeMap;
  protected Set<String> closedList;
  protected Map<String, Double> gScoreMap;
  protected Map<String, Edge> cameFrom;
  @Getter
  protected int numberOfExpandedNodes;
  protected Comparator<Double> comparator;
  //用于动态获取当前小车的运行路径和轨迹
  private final ReservationPool reservationPool;
  private Graph<String, Edge> graph;
  @Setter
  private String vehicleName;
  /**
   * 这是用来进行更新动态权值的参数。
   */
  private Map<String, Double> gWight;

  public FSYAStartShortPathAlg(Graph<String, Edge> graph, ReservationPool reservationPool) {
    this(graph, PairingHeap::new, reservationPool);
  }

  public FSYAStartShortPathAlg(Graph<String, Edge> graphSupplier, Supplier<AddressableHeap<Double, String>> heapSupplier,
                               ReservationPool reservationPool) {
    this.comparator = new ToleranceDoubleComparator();
    this.heapSupplier = Objects.requireNonNull(heapSupplier, "Heap supplier cannot be null!");
    this.graph = graphSupplier;
    this.reservationPool = reservationPool;
  }

  private void initialize() {
    this.openList = this.heapSupplier.get();
    this.vertexToHeapNodeMap = new HashMap<>();
    this.closedList = new HashSet<>();
    this.gScoreMap = new HashMap<>();
    this.cameFrom = new HashMap<>();
    this.numberOfExpandedNodes = 0;
    gWight = new HashMap<>();
  }

  @Override
  public GraphPath<String, Edge> getPath(String sourceVertex, String targetVertex) {
    if (this.graph.containsVertex(sourceVertex) && this.graph.containsVertex(targetVertex)) {
      if (sourceVertex.equals(targetVertex)) {
        vehicleName = null;
        return this.createEmptyPath(sourceVertex, targetVertex);
      } else {
        this.initialize();
        this.gScoreMap.put(sourceVertex, 0.0);
        AddressableHeap.Handle<Double, String> heapNode = this.openList.insert(0.0, sourceVertex);
        this.vertexToHeapNodeMap.put(sourceVertex, heapNode);

        do {
          AddressableHeap.Handle<Double, String> currentNode = this.openList.deleteMin();
          if (currentNode.getValue().equals(targetVertex)) {
            vehicleName = null;
            return this.buildGraphPath(sourceVertex, targetVertex, currentNode.getKey());
          }

          this.expandNode(currentNode, targetVertex);
          this.closedList.add(currentNode.getValue());
        } while (!this.openList.isEmpty());
        vehicleName = null;
        return this.createEmptyPath(sourceVertex, targetVertex);
      }
    } else {
      vehicleName = null;
      throw new IllegalArgumentException("Source or target vertex not contained in the graph!");
    }
  }

  private void expandNode(AddressableHeap.Handle<Double, String> currentNode, String endVertex) {
    ++this.numberOfExpandedNodes;
    Set<Edge> outgoingEdges = this.graph.outgoingEdgesOf(currentNode.getValue());

    for (Edge edge : outgoingEdges) {
      //获取这条边的两个顶点的指定顶点的另外那个顶点
      String successor = Graphs.getOppositeVertex(this.graph, edge, currentNode.getValue());
      if (!successor.equals(currentNode.getValue())) {
        double gScore = this.gScoreMap.get(currentNode.getValue());
        double tentativeGScore = gScore + this.graph.getEdgeWeight(edge);
        double fScore = tentativeGScore + getCostEstimate(edge);
        if (this.vertexToHeapNodeMap.containsKey(successor)) {
          if (!(tentativeGScore >= this.gScoreMap.get(successor))&&fScore < this.vertexToHeapNodeMap.get(successor).getKey()) {
            this.cameFrom.put(successor, edge);
            this.gScoreMap.put(successor, tentativeGScore);
            if (this.closedList.contains(successor)) {
              this.closedList.remove(successor);
              this.openList.insert(fScore, (this.vertexToHeapNodeMap.get(successor)).getValue());
            } else {
              (this.vertexToHeapNodeMap.get(successor)).decreaseKey(fScore);
            }
          }
        } else {
          this.cameFrom.put(successor, edge);
          this.gScoreMap.put(successor, tentativeGScore);
          AddressableHeap.Handle<Double, String> heapNode = this.openList.insert(fScore, successor);
          this.vertexToHeapNodeMap.put(successor, heapNode);
        }
      }
    }

  }

  /**
   * 这里是以当前节点为起点的的路径(排除前驱节点与当前节点的路线，防止循环探索)
   *
   * @param edge 当前需要判断的边
   * @return
   */
  private double getCostEstimate(Edge edge) {
    //如果没有附加权值--（退化成dj算法）
    if (vehicleName == null)
      return 0;
    Map<String, Integer> claimsMaps = reservationPool.getClaimsMaps(vehicleName);
    Set<String> blockSources = reservationPool.getBlockSourceByVehicleName(vehicleName);
    Edge edge1 = graph.getEdge(edge.getTargetVertex(), edge.getSourceVertex());
    if (edge1 != null && (claimsMaps.containsKey(edge1.getPath().getName()) || blockSources.contains(edge.getPath().getName()))) {
      return Double.MAX_VALUE;
    }
    double result = 0.0;
    result += claimsMaps.getOrDefault(edge.getPath().getName(), 0) * 1000;
    return result;

  }

  private GraphPath<String, Edge> buildGraphPath(String startVertex, String targetVertex, double pathLength) {
    List<Edge> edgeList = new ArrayList<>();
    List<String> vertexList = new ArrayList<>();
    vertexList.add(targetVertex);
    String v = targetVertex;

    while (!v.equals(startVertex)) {
      edgeList.add(this.cameFrom.get(v));
      v = Graphs.getOppositeVertex(this.graph, this.cameFrom.get(v), v);
      vertexList.add(v);
    }

    Collections.reverse(edgeList);
    Collections.reverse(vertexList);
    return new GraphWalk<>(this.graph, startVertex, targetVertex, vertexList, edgeList, pathLength);
  }


  public SingleSourcePaths<String, Edge> getPaths(String source) {
    if (!this.graph.containsVertex(source)) {
      throw new IllegalArgumentException("graph must contain the source vertex");
    } else {
      Map<String, GraphPath<String, Edge>> paths = new HashMap<>();

      for (String v : this.graph.vertexSet()) {
        paths.put(v, this.getPath(source, v));
      }

      return new ListSingleSourcePathsImpl<>(this.graph, source, paths);
    }
  }

  public double getPathWeight(String source, String sink) {
    GraphPath<String, Edge> p = this.getPath(source, sink);
    return p == null ? Double.POSITIVE_INFINITY : p.getWeight();
  }

  protected final GraphPath<String, Edge> createEmptyPath(String source, String sink) {
    return source.equals(sink) ? GraphWalk.singletonWalk(this.graph, source, 0.0) : null;
  }

  @Override
  public GraphPath<String, Edge> getPath(String var1, String var2, Map<String, Double> weights) {
    this.gWight = weights;
    return getPath(var1, var2);
  }

}
